/**
 * @file face_detect.cc
 * @author your name (you@domain.com)
 * @brief 人脸检测相关事务
 * @version 0.1
 * @date 2022-09-17
 * 
 * @copyright Copyright (c) 2022
 * 
 */

#include "face_detect.h"
#include "imageProcess.h"
#include "object_detect.h"
#include <semaphore.h>              // sem_init()
extern mutex mtxQueueReadImage_368;
extern queue<pair<pair<string,Mat>,Mat>> queueReadImage_368;
extern int target_368;
sem_t binSem_1;
//送入单张检测
//不仅需要进行人脸检测，而且要判断人脸是否处于主驾驶位上
static bool detect(Mat img, CascadeClassifier cascade)
{
    vector<cv::Rect> faces;
    cascade.detectMultiScale(img, faces, 1.2, 2);
    //Todo:人脸的位置不在主驾驶位，也认为是无人    
    if(faces.size() == 0)      
    {
        return false;
    }
    else 
    {
        for(int i = 0; i< faces.size(); i++)
        {
            float x_center = faces[i].x + faces[i].width * 0.5;
            // cout << x_center << endl;
            if( x_center > 120 && x_center < 300)
                return true;
        }
    }
    return false;
}

/*-------------------------------------------
                线程2:人脸检测
-------------------------------------------*/
int face_detector(int cpu_id)
{
    struct timeval start_time, stop_time;
    gettimeofday(&start_time, NULL);
    //线程对cpu亲和性设定   
    cpu_set_t mask;
    CPU_ZERO(&mask);
    CPU_SET(cpu_id, &mask);
    if (pthread_setaffinity_np(pthread_self(), sizeof(mask), &mask) < 0)
    {
        cerr << "set thread affinity failed" << endl;
        return -1;
    }
    printf("Bind face_detector thread to CPU %d\n", cpu_id);

    //定义并使用人脸检测
    cv::CascadeClassifier cascade;
    string cascadeName = "model/haarcascade_frontalface_default.xml";
    if( !cascade.load( cascadeName ))
    {
        cerr << "ERROR: Could not load classifier cascade" << endl;
        return -1;
    }  
    int ret;
    ret = sem_init(&binSem_1, 0, 0);
    if (ret) {
        printf("Semaphore initialization failed!!\n");
        return -1;
    }

    //2号线程可能比主线程来的快  
    while(target_368 == 0)
    {
        usleep(1000);
    }
    cout <<"在data目录下共找到"<< target_368 <<"个jpg文件"<<endl;
    //测试从main线程传递来的数据  
    int n = target_368;
    int status = -1;
    Mat resimg_368;
    array<thread, 1> threads;
    int flag = 0;           //指示线程2有没有开启
    int cnt = 0;           //未检出人脸计数器
    while(n > 0){
        //如果线程3开启，阻塞当前线程；等待线程3结束完目标检测，再开始人脸的本轮检测
        if(flag == 1)
            sem_wait(&binSem_1);
        
        //每隔2s的人脸检测
        n = n-10;
        if(n < 0)   break;
        resimg_368 = select_img_from_368_queue(10);
        
        //如果每隔2s检测到
        //Todo: 每隔2s到每隔0.5s的切换没有必要，连续两个2s一检检测结果为正即可判定有人；有人 - 无人判断，连续5个2s一检无人即可。
        if(status != NORMALDRING && detect(resimg_368, cascade)){
            int nums = 0;
            for(int i = 0; i< 4; i++){
                //每隔400ms检测一张，检测四次
                resimg_368 = select_img_from_368_queue(2);
                n = n - 2;
                if(detect(resimg_368, cascade)) nums+=1;
                //如果四次有三次，切换到司机正常驾驶状态
                if(nums >= 3)
                {
                    status = NORMALDRING;
                    threads = {thread(object_detect, 3, n)};          //这里同步的n 是队列还剩多少张
                    flag = 1;
                    break;
                }
            }
        }
        else if(!detect(resimg_368, cascade)){
            //如果没有检测到人脸
            cnt++;
            if(cnt == 2){
            //说明连续10秒钟没有检测到人脸了,切换到无人状态
            //Todo:从10s改成了4s,需要改回注意
            cnt = 0;
            status = NODRIVER;
            flag = 0;
            //关闭23线程
            pthread_t rt=threads[0].native_handle();
            pthread_cancel(rt);
            cout << "目标检测线程已关闭" << endl;
            }
        }
        else{
            //如果检测到一次人脸，未检出人脸计数器置零
            cnt = 0;
        }
        printf("2号线程输出 %d\n",status);
    }

    gettimeofday(&stop_time, NULL);
    printf(" 线程2:人脸检测总共用时： %f ms\n",(__get_us(stop_time) - __get_us(start_time)) / 1000);
    threads[0].join(); 
    return 0;
}

